#include "node_fps.hh"

NodeFps::NodeFps (unsigned int step)
  : size_ (step)
  , son_ (0)
{
  if (step != 16)
    {
      son_ = new NodeFps*[8];
      for (int i = 0; i < 8; ++i)
	son_[i] = new NodeFps (step / 2);
    }
}

NodeFps::~NodeFps ()
{
  if (son_)
    {
      for (int i = 0; i < 8; ++i)
	delete son_[i];
      delete[] son_;
    }
}

void
NodeFps::add_obj (Object* obj, Vector3d& pos)
{
  Vector3f pos_obj = obj->pos_get ();
  Vector3f size = obj->size_get ();

  bool sup = false;
  bool inf = false;

  int node = 0;
  bool end = false;

  Vector3d new_pos = pos;
  if (son_)
    {
      for (int i = 0; i < 3 && !end; ++i)
	{
	  sup = (pos_obj (i) + size (i)) > pos (i) - 1;
	  inf = (pos_obj (i) - size (i)) > pos (i) - 1;

	  if (inf != sup)
	    end = true;
	  else
	    {
	      if (inf)
		new_pos (i, new_pos (i) + pos (i) / 2);
	      else
		new_pos (i, new_pos (i) - pos (i) / 2);

	      node = node * 2 + inf;
	    }
	}

      if (end)
	obj_.push_back (obj);
      else
	son_[node]->add_obj (obj, new_pos);
    }
  else
    obj_.push_back (obj);
}

bool
NodeFps::delete_obj (Object* obj, Vector3d& pos)
{
  Vector3f pos_obj = obj->pos_get ();
  Vector3f size = obj->size_get ();

  bool sup = false;
  bool inf = false;

  int node = 0;
  bool end = false;

  Vector3d new_pos = pos;
  if (son_)
    {

      for (int i = 0; i < 3 && !end; ++i)
	{
	  sup = (pos_obj (i) + size (i)) > pos (i) - 1;
	  inf = (pos_obj (i) - size (i)) > pos (i) - 1;

	  if (inf != sup)
	    end = true;
	  else
	    {
	      if (inf)
		new_pos (i, new_pos (i) + pos (i) / 2);
	      else
		new_pos (i, new_pos (i) - pos (i) / 2);

	      node = node * 2 + inf;
	    }
	}

      if (end)
	{
	  std::list<Object*>::iterator it = find (obj_.begin (), obj_.end (), obj);
	  if (it != obj_.end ())
	    {
	      obj_.erase (it);
	      return true;
	    }
	  return false;
	}
      else
	return (son_[node]->delete_obj (obj, new_pos));
    }
  else
    {
      std::list<Object*>::iterator it = find (obj_.begin (), obj_.end (), obj);
      if (it != obj_.end ())
	{
	  obj_.erase (it);
	  return true;
	}
      return false;
    }
}

void
NodeFps::accept (PhysicFunctor* phy, Vector3d& pos)
{
  Vector3f pos_obj = phy->object_get ()->pos_get ();
  Vector3f size = phy->object_get ()->size_get ();

  bool sup = false;
  bool inf = false;

  int node = 0;
  bool end = false;

  Vector3d new_pos = pos;

  (*phy) (obj_);

  if (son_)
    {
      for (int i = 0; i < 3 && !end; ++i)
	{
	  sup = (pos_obj (i) + size (i)) > pos (i) - 1;
	  inf = (pos_obj (i) - size (i)) > pos (i) - 1;

	  if (inf != sup)
	    end = true;
	  else
	    {
	      if (inf)
		new_pos (i, new_pos (i) + pos (i) / 2);
	      else
		new_pos (i, new_pos (i) - pos (i) / 2);

	      node = node * 2 + inf;
	    }
	}

      if (end)
	for (int i = 0; i < 8; ++i)
	  son_[i]->accept_all (phy);
      else
	son_[node]->accept (phy, new_pos);
    }
}

void
NodeFps::accept (DisplayFunctor* disp)
{
  List<Object*>* list = 0;
  List<Object*>* tmp = 0;
  List<Object*>* head = 0;

  // list initialization
  for (std::list<Object*>::iterator it = obj_.begin ();
       it != obj_.end ();
       ++it)
    {
      list = new List<Object*>;
      list->elt = *it;

      if (tmp)
	tmp->next = list;
      else
	head = list;
      tmp = list;
    }

  (*disp) (head);
  list = head;
  // list destruction
  while (list != 0)
    {
      tmp = list;
      list = list->next;
      delete tmp;
    }

  if (son_)
    for (int i = 0; i < 8; ++i)
      son_[i]->accept (disp);
    
}

void
NodeFps::accept_all (PhysicFunctor* phy)
{
  (*phy) (obj_);

  if (son_)
    for (int i = 0; i < 8; ++i)
      son_[i]->accept_all (phy);
}
